

#include "RWCamera.h"



RWCamera::RWCamera()
{

}
RWCamera::~RWCamera()
{

}

void RWCamera::init(webots::Robot* R, ros::NodeHandle* n_in,string name)
{
  n = n_in;
  time_step = R->getBasicTimeStep();
  stringstream ss1, ss2;
  ss2 << name;
  //ACTIVE RGB CAMERA
  RGBCAM = R->getCamera(ss2.str());
  RGBCAM->enable(time_step);
  rgbcam_width = RGBCAM->getWidth();
  rgbcam_height = RGBCAM->getHeight();

  //ROS PUBLISHER CREATION
  std::stringstream RCImage,RMImage;
  //Publisher names
  string RobotName = R->getName();
  RCImage << RobotName << "/"<<ss2.str()<<"/rgb/image_color";
  RMImage << RobotName << "/"<<ss2.str()<<"/rgb/image_mono";


  rgbcam_image_color_pub = n->advertise<sensor_msgs::Image>(RCImage.str(), 1000);
  rgbcam_image_mono_pub = n->advertise<sensor_msgs::Image>(RMImage.str(), 1000);

  //ROS MESSAGES

  RgbcamImageColor.header.seq = 0;
  RgbcamImageColor.header.frame_id = "rgbcam";  //CHANGE!!!!!!
  RgbcamImageColor.height = rgbcam_height;
  RgbcamImageColor.width = rgbcam_width;
  RgbcamImageColor.encoding = "RGB8";
  RgbcamImageColor.is_bigendian = 0; //COMPROBAR CON DATOS REALES???
  RgbcamImageColor.step = rgbcam_width * 3; //COMPROBAR QUE VA EN LAS FILAS
  RgbcamImageColor.data.resize(rgbcam_height*rgbcam_width*3);


  RgbcamImageMono.header.seq = 0;
  RgbcamImageMono.header.frame_id = "rgbcam";  //CHANGE!!!!!!
  RgbcamImageMono.height = rgbcam_height;
  RgbcamImageMono.width = rgbcam_width;
  RgbcamImageMono.encoding = "MONO8";  //Comprobar con kinect real
  RgbcamImageMono.is_bigendian = 0; //COMPROBAR CON DATOS REALES???
  RgbcamImageMono.step = rgbcam_width; //COMPROBAR QUE VA EN LAS FILAS
  RgbcamImageMono.data.resize(rgbcam_height*rgbcam_width);
}

void RWCamera::iterate()
{
  ros::Time now = ros::Time::now();
 //COLOR IMAGE
  const unsigned char* rgbcam_image_color = RGBCAM->getImage();
  memcpy(RgbcamImageColor.data.data(),rgbcam_image_color,rgbcam_height*rgbcam_width*3);
  RgbcamImageColor.header.seq++;
  RgbcamImageColor.header.stamp = now;
  //MONO IMAGE
  unsigned char rgbcam_image_mono[(int)(rgbcam_height*rgbcam_width)];
  for (int k = 0;k< rgbcam_height*rgbcam_width;k++)
  {
    rgbcam_image_mono[k] = (unsigned char)((rgbcam_image_color[3*k]+rgbcam_image_color[3*k+1]+rgbcam_image_color[3*k+2]))/3;
  }
  memcpy(RgbcamImageMono.data.data(),rgbcam_image_mono,rgbcam_height*rgbcam_width);
  RgbcamImageMono.header.seq++;
  RgbcamImageMono.header.stamp = now;
  //PUBLISH MESSAGES
  rgbcam_image_color_pub.publish(RgbcamImageColor);
  rgbcam_image_mono_pub.publish(RgbcamImageMono);

}


